#!/usr/bin/python3

import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, PoseStamped
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray, Bool
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
import copy
from tf2_geometry_msgs import tf2_geometry_msgs

class real_sim_mapping(object):
    def __init__(self,nodename):
        rospy.init_node(nodename)
        #debug_prefix = "/fine_machining/"
        debug_prefix = ""

        self.real_joint_states = rospy.Subscriber(debug_prefix + "cmd_joint_states",JointState,self.realJointStates_cb)
        self.turn_on_realtime_tracking = rospy.Subscriber("turn_on_realtime_tracking",Bool, self.turnOnRealtimeTracking_cb)
        self.sim_ptp_controller_command = rospy.Publisher(debug_prefix + 'ptp_controller/command', Float64MultiArray, queue_size=10)

        self.joint_name_index = None
        self.realtime_tracking_switch_on = True

    def realJointStates_cb(self,msg):
        if(not self.realtime_tracking_switch_on):
            return 

        if(self.joint_name_index is None):
            para_name = "ptp_controller/joints"
            if(rospy.has_param(para_name)):
                self.joint_names = rospy.get_param(para_name)
                self.joint_name_index = {self.joint_names[i]:i for i in range(len(self.joint_names))}
            else:
                return

        sent_command=[0.0]*len(self.joint_names)
        for i in range(len(msg.name)):
            sent_command[self.joint_name_index[msg.name[i]]] = msg.position[i]
        
        self.sim_ptp_controller_command.publish(Float64MultiArray(data=sent_command))
        return

    def turnOnRealtimeTracking_cb(self,msg):
        print(msg)
        return

    def run(self):
        '''
        rate = rospy.Rate(50)
        trans = TransformStamped()
        while not rospy.is_shutdown():
            if(self.msgNames is None or self.msgPose is None):
                rate.sleep()
                continue

            for i in range(len(self.msgNames)):
                if(self.msgNames[i] in self.baseLinks.keys()):
                    #print(msg.name[i])
                    #print(msg.pose[i])
                    trans.header.frame_id = "world"
                    trans.child_frame_id = self.baseLinks[self.msgNames[i]]

                    trans.header.stamp = rospy.Time.now()

                    trans.transform.translation.x = self.msgPose[i].position.x
                    trans.transform.translation.y = self.msgPose[i].position.y
                    trans.transform.translation.z = self.msgPose[i].position.z

                    trans.transform.rotation.x = self.msgPose[i].orientation.x
                    trans.transform.rotation.y = self.msgPose[i].orientation.y
                    trans.transform.rotation.z = self.msgPose[i].orientation.z
                    trans.transform.rotation.w = self.msgPose[i].orientation.w

                    self.tfBroadcaster.sendTransform(trans)
            rate.sleep()
        '''
        rospy.spin()

if __name__ == '__main__':
    service = real_sim_mapping('real_sim_mapping')
    service.run()
